#include <system.h>
#include <std.h> 
#include <pio.h>
#include "types.h"
#include <error.h>
#include <memory.h>

typedef struct
{
    ulong fpu:1;
    ulong vme:1;
    ulong de:1;
    ulong pse:1;
    ulong tsc:1;
    ulong msr:1;
    ulong pae:1;
    ulong mce:1;
    ulong cx8:1;
    ulong apic:1;
    ulong ff_res0:1;
    ulong sep:1;
    ulong mtrr:1;
    ulong pge:1;
    ulong mca:1;
    ulong cmov:1;
    ulong pat:1;
    ulong pse_36:1;
    ulong ff_res1:5;
    ulong mmx:1;
    ulong fxsr:1;
    ulong sse:1;
    ulong ff_res2:5;
    ulong amd_3dnow:1;
} NOALIGN cpuid_flags_type;

typedef struct
{
    ulong cpuid;
    const char *name;
    ulong signature[0];
    ulong stepping:4;
    ulong model:4;
    ulong family:4;
    ulong type:2;
    ulong s_res0:18;
    union
    {
	ulong real_flags;
	cpuid_flags_type flags;
    } flags;
    ulong configuration;
    union
    {
        ulong num;
        unsigned long id : 8;
    } brand;
} NOALIGN cpu_info_type;

typedef struct
{
    char name[64];
    const char *vendor;
    ulong speed;
} NOALIGN parsed_cpu_type;

typedef struct
{
    ulong vendor;
    ulong x86;
    const char *name[32];
} cpu_model_type;

void init_cpu(void);

#define _countof(a)	( sizeof(a) / sizeof((a)[0]) )

struct regs {
    ulong eax;
    ulong ebx;
    ulong ecx;
    ulong edx;
};

ulong getPhysicalMemoryMB( void )
{
	return 32;
}

cpu_info_type cpu_info;
parsed_cpu_type parsed_cpu;
static const char *cpu_name_nocpuid = "noname";
static char cpuid_string[ 13 ];
ulong hz = 100;
ulong mem_end;

// CPUID operations.
enum
{
	// Get the cpu vendor string.
	GET_CPU_VENDOR	= 0,
	// Get cpu informations.
	GET_CPU_INFO	= 1,
	// Get cpu cache informations.
	GET_CPU_CACHE	= 2,
	// Check if the brand string is available.
	CPU_BRAND_STRING_IS_AVAILABLE	= 0x80000000,
	// Get 1st part of the brand string.
	CPU_GET_BRAND_STRING1		= 0x80000002,
	// Get 1st part of the brand string.
	CPU_GET_BRAND_STRING2		= 0x80000003,
	// Get 1st part of the brand string.
	CPU_GET_BRAND_STRING3		= 0x80000004,
};

// CPU names.
typedef struct cpu_name
{
	int family;
	char *model_names[16];
} cpu_name_t;

// All the string names for Intel CPU models.
static cpu_name_t intel_cpu_names[] =
{
	{
		.family = 4, .model_names =
		{
			[ 0 ] = "486 DX-25/33",
			[ 1 ] = "486 DX-50",
			[ 2 ] = "486 SX",
			[ 3 ] = "486 DX/2",
			[ 4 ] = "486 SL",
			[ 5 ] = "486 SX/2",
			[ 7 ] = "486 DX/2-WB",
			[ 8 ] = "486 DX/4",
			[ 9 ] = "486 DX/4-WB",
		}
	},
	{
		.family = 5, .model_names =
		{
			[ 0 ] = "Pentium 60/66 A-step",
			[ 1 ] = "Pentium 60/66",
			[ 2 ] = "Pentium 75-200",
			[ 3 ] = "Overdrive PODP5V83",
			[ 4 ] = "Pentium MMX",
			[ 7 ] = "Mobile Pentium 75-200",
			[ 8 ] = "Mobile Pentium MMX",
		}
	},
	{
		.family = 6, .model_names =
		{
			[ 0  ] = "Pentium Pro A-step",
			[ 1  ] = "Pentium Pro",
			[ 3  ] = "Pentium II (Klamath)",
			[ 4  ] = "Pentium II (Deschutes)",
			[ 5  ] = "Pentium II (Deschutes)",
			[ 6  ] = "Mobile Pentium II",
			[ 7  ] = "Pentium III (Katmai)",
			[ 8  ] = "Pentium III (Coppermine)",
			[ 10 ] = "Pentium III (Cascades)",
			[ 11 ] = "Pentium III (Tualatin)",
		}
	},
	{
		.family = 15, .model_names =
		{
			[ 0 ] = "Pentium 4 (Unknown)",
			[ 1 ] = "Pentium 4 (Willamette)",
			[ 2 ] = "Pentium 4 (Northwood)",
			[ 4 ] = "Pentium 4 (Foster)",
			[ 5 ] = "Pentium 4 (Foster)",
		}
	},
};

// All the string names for AMD CPU models.
static cpu_name_t amd_cpu_names[] =
{
	{
		.family = 4, .model_names =
		{
			[ 3  ] = "486 DX/2",
			[ 7  ] = "486 DX/2-WB",
			[ 8  ] = "486 DX/4",
			[ 9  ] = "486 DX/4-WB",
			[ 14 ] = "Am5x86-WT",
			[ 15 ] = "Am5x86-WB",
		}
	},
	{
		.family = 5, .model_names =
		{
			[ 0 ] = "K5/SSA5",
			[ 1 ] = "K5",
			[ 2 ] = "K5",
			[ 3 ] = "K5",
			[ 6 ] = "K6",
			[ 7 ] = "K6",
			[ 8 ] = "K6-2",
			[ 9 ] = "K6-3",
		}
	},
	{
		.family = 6, .model_names =
		{
			[ 0 ] = "Athlon",
			[ 1 ] = "Athlon",
			[ 2 ] = "Athlon",
			[ 4 ] = "Athlon",
		}
	},
};

#define BIT_VALUE( x ) ( 1 << ( x ) )
#define latch ( ( 1193180 + hz / 2 ) / hz )
#define calibrate_latch ( 5 * latch )
#define calibrate_time ( 5 * 1000020 / hz )

enum {
    FLAG_CARRY				= ( BIT_VALUE(  0 ) ),
    FLAG_SET				= ( BIT_VALUE(  1 ) ),
    FLAG_PARITY				= ( BIT_VALUE(  2 ) ),
    FLAG_ADJUST				= ( BIT_VALUE(  4 ) ),
    FLAG_ZERO				= ( BIT_VALUE(  6 ) ),
    FLAG_SIGN				= ( BIT_VALUE(  7 ) ),
    FLAG_TRAP				= ( BIT_VALUE(  8 ) ),
    FLAG_INTERRUPT_ENABLE		= ( BIT_VALUE(  9 ) ),
    FLAG_DIRECTION			= ( BIT_VALUE( 10 ) ),
    FLAG_OVERFLOW			= ( BIT_VALUE( 11 ) ),
    FLAG_IOPL_LOW			= ( BIT_VALUE( 12 ) ),
    FLAG_IOPL_HIGH			= ( BIT_VALUE( 13 ) ),
    FLAG_NESTED_TASK			= ( BIT_VALUE( 14 ) ),
    FLAG_RESUME_TASK			= ( BIT_VALUE( 16 ) ),
    FLAG_V8086_MODE			= ( BIT_VALUE( 17 ) ),
    FLAG_ALIGNMENT_CHECK		= ( BIT_VALUE( 18 ) ),
    FLAG_VIRTUAL_INTERRUPT		= ( BIT_VALUE( 19 ) ),
    FLAG_VIRTUAL_INTERRUPT_PENDING	= ( BIT_VALUE( 20 ) ),
    FLAG_ID				= ( BIT_VALUE( 21 ) )
};

static inline void rdtsc( ulong *low, ulong *high ) {
    asm volatile( "rdtsc" : "=a" ( *low ), "=d" ( *high ) );
}

static inline void cpuid(
    ulong command, ulong *eax, ulong *ebx, ulong *ecx, ulong *edx ) {
    asm volatile( "cpuid" : "=a" ( *eax ), "=b" ( *ebx ), "=c" ( *ecx ),
	"=d" ( *edx ) : "a" ( command ) : "cc" );
}

static int cpuid_found( void ) {
    int retval;

    asm volatile(
	"pushl %%ebx;" \
	"pushfl;" \
	"xorl %1,(%%esp);" \
	"popfl;" \
	"pushfl;" \
	"movl (%%esp),%%eax;" \
	"andl %1,%%eax;" \
	"popfl;" \
	"pushfl;" \
	"xorl %1,(%%esp);" \
	"popfl;" \
	"pushfl;" \
	"movl (%%esp),%%ebx;" \
	"andl %1,%%ebx;" \
	"popfl;" \
	"cmpl %%ebx,%%eax;" \
	"jne 1f;" \
	"movl $0,%%eax;" \
	"jmp 2f;" \
	"1: movl $1,%%eax;" \
	"2: popl %%ebx;" : "=&a" ( retval ) :"g" ( FLAG_ID ) );
    return retval;
}

static int is_486( void ) {
    int retval;

    asm (
	"pushl %%ecx;" \
	"pushfl;" \
	"popl %%eax;" \
	"movl %%eax, %%ecx;" \
	"xorl $0x40000, %%eax;" \
	"pushl %%eax;" \
	"popf;" \
	"pushf;" \
	"popl %%eax;" \
	"xorl %%ecx, %%eax;" \
	"and $0x40000, %%eax;" \
	"je 1f;" \
	"movl $1, %%eax;" \
	"jmp 2f;" \
	"1: movl $0, %%eax;" \
	"2: popl %%ecx;" : "=&a" ( retval ) );
    return retval;
}

static ulong calibrate_tsc( void ) {
    ulong startlow, starthigh, endlow, endhigh, count;
    
    outb( 0x61, ( inb( 0x61 ) & ~0x02 ) | 0x01 );
    outb( 0x43, 0xb0 );
    outb( 0x42, calibrate_latch & 0xff );
    outb( 0x42, calibrate_latch >> 8 );
    rdtsc( &startlow, &starthigh );
    count = 0;
    do {
	count++;
    } while ( ( inb( 0x61 ) & 0x20 ) == 0 );
    rdtsc( &endlow, &endhigh );
    if ( count <= 1 ) {
	return 0;
    }
    asm ( "subl %2, %0\n"
	"sbbl %3,%1" : "=a" ( endlow ), "=d" ( endhigh ) :
	"g" ( startlow ), "g" ( starthigh ), "0" ( endlow ), "1" ( endhigh ) );
    if ( endhigh != 0 ) {
	return 0;
    }
    if ( endlow <= calibrate_time ) {
	return 0;
    }
    asm ( "divl %2" : "=a" ( endlow ), "=d" ( endhigh ) :
	"r" ( endlow ),"0" ( 0 ), "1" ( calibrate_time ) );
    return endlow;
}

// Get the CPU speed
static ulong speed( void ) {
    ulong tsc_quotient = calibrate_tsc();
    ulong cpu_hz = 0;

    if ( tsc_quotient != 0 ) {
	ulong eax = 0, edx = 1000000;
	asm ( "divl %2" : "=a" ( cpu_hz ), "=d" ( edx ) :
	"r" ( tsc_quotient ), "0" ( eax ), "1" ( edx ) );
    }
    return cpu_hz / 1000000;
}

int fpu_test( void ) {
    asm (
	"finit\n"
	/* "movl $0x5A5A, %%eax\n"
	"fnstsw %%eax\n"
	"orl %%eax, %%eax\n"
	"jz 1f\n"
	"movl $1, %%eax\n"
	"1:\n"
	"notl %%eax\n"
	: "=a" ( rc ) :*/ );

    return 0;
}

// Get the cpu brand string if it is available.
int cpu_get_brandstring( parsed_cpu_type *cpu )
{
	ulong *s = (ulong *)cpu->name;
	char *p, *q;
	ulong dummy, eax;

	// Check if brand string is available.
	cpuid( CPU_BRAND_STRING_IS_AVAILABLE,
		&eax, &dummy, &dummy, &dummy);
	if( eax < 0x80000004 )
	{
		// Brand string not supported! Zero the cpu name.
		memset( cpu->name, 0, sizeof(cpu->name) );
		return( -1 );
	}

	// Get the cpu brand string.
	cpuid( CPU_GET_BRAND_STRING1, &s[0], &s[1], &s[2], &s[3] );
	cpuid( CPU_GET_BRAND_STRING2, &s[4], &s[5], &s[6], &s[7] );
	cpuid( CPU_GET_BRAND_STRING3, &s[8], &s[9], &s[10], &s[11] );
	cpu->name[48] = '\0';

	// Intel chips right justify the string.
	// Left justify this string.
	p = q = &cpu->name[0];
	while ( *p == ' ' )
		p++;
	if ( p != q )
	{
		while ( *p )
			*q++ = *p++;
		// Zero pad the rest.
		while ( q <= &cpu->name[48] )
			*q++ = '\0';
	}

	// Well done.
	return( 0 );
}

// Get the cpu model name.
int cpu_get_modelname( void )
{
	cpu_name_t *names;
	int count, i;

	// First of all try to get the brand string.
	if( cpu_info.cpuid == 1 && cpu_get_brandstring(&parsed_cpu) == 0 )
	{
		// Well done, brand string supported.
		return( 0 );
	}

	// Find the vendor of the CPU.
	if( strcmp(parsed_cpu.vendor, "GenuineIntel") == 0 )
	{
		// Try to identify the cpu name with the brand ID.
		// This is useful to identify Celeron and Xeon
		// processor.
		switch( cpu_info.brand.id )
		{
			case 1:
			case 10:
				strncpy( parsed_cpu.name,
					"Intel(R) Celeron(R)",
					sizeof(parsed_cpu.name) );
				return( 0 );
			break;

			case 3:
				if( cpu_info.signature[0] == 0x6b1 )
					strncpy( parsed_cpu.name,
						"Intel(R) Celeron(R)",
						sizeof(parsed_cpu.name) );
				else
					strncpy( parsed_cpu.name,
						"Intel(R) Pentium(R) Xeon",
						sizeof(parsed_cpu.name) );
				return( 0 );
			break;

			case 6:
				strncpy( parsed_cpu.name,
					"Mobile Intel(R) Pentium(R) III",
					sizeof(parsed_cpu.name) );
				return( 0 );
			break;

			case 7:
			case 15:
				strncpy( parsed_cpu.name,
					"Mobile Intel(R) Celeron(R)",
					sizeof(parsed_cpu.name) );
				return( 0 );
			break;

			case 11:
				if( cpu_info.signature[0] < 0xf13 )
					strncpy( parsed_cpu.name,
						"Intel(R) Xeon MP",
						sizeof(parsed_cpu.name) );
				else
					strncpy( parsed_cpu.name,
						"Intel(R) Xeon",
						sizeof(parsed_cpu.name) );
				return( 0 );
			break;

			case 12:
				strncpy( parsed_cpu.name,
					"Intel(R) Xeon MP",
					sizeof(parsed_cpu.name) );
				return( 0 );
			break;

			case 13:
				if( cpu_info.signature[0] < 0xf13 )
					strncpy( parsed_cpu.name,
						"Intel(R) Xeon",
						sizeof(parsed_cpu.name) );
				else
					strncpy( parsed_cpu.name,
						"Mobile Intel(R) Pentium 4",
						sizeof(parsed_cpu.name) );
			break;
		}
		// Find the cpu name in the intel list.
		names = intel_cpu_names;
		count = _countof( intel_cpu_names );
	}
	else if( strcmp(parsed_cpu.vendor, "AuthenticAMD") == 0 )
	{
		// Find the cpu name in the amd list.
		names = amd_cpu_names;
		count = _countof( amd_cpu_names );
	}
	else
	{
		switch( cpu_info.family )
		{
			case 3:
			strncpy( parsed_cpu.name, "386", sizeof(parsed_cpu.name) );
			break;

			case 4:
			strncpy( parsed_cpu.name, "486", sizeof(parsed_cpu.name) );
			break;

			default:
			strncpy( parsed_cpu.name, "Unknown", sizeof(parsed_cpu.name) );
			break;
		}
		return( -1 );
	}

	// Get the right CPU model name.
	for( i = 0; i < count; i++ )
	{
		if( cpu_info.family == names[i].family )
		{
			int id = cpu_info.model;
			// Copy the cpu name.
			strncpy( parsed_cpu.name,
				names[i].model_names[ id ],
				sizeof(parsed_cpu.name) );
		}
	}

	return( 0 );
}

static void getcpuinfo( void ) {
    if ( cpuid_found() ) {
	ulong dummy;

	cpu_info.cpuid = 1;
	cpuid( GET_CPU_VENDOR, &dummy, ( ulong * ) &cpuid_string[ 0 ],
	    ( ulong * ) &cpuid_string[ 8 ], ( ulong * ) &cpuid_string[ 4 ] );
	cpu_info.name = cpuid_string;
	cpuid( GET_CPU_INFO, ( ulong * ) &cpu_info.signature, ( ulong * )&cpu_info.brand.num,
	    &dummy, ( ulong * ) &cpu_info.flags.flags );
	if ( ( cpu_info.cpuid & cpu_info.flags.flags.tsc ) != 0 ) {
	    parsed_cpu.speed = speed();
	}
    } else {
	cpu_info.name = cpu_name_nocpuid;
	cpu_info.cpuid = 0;
	if ( is_486() ) {
	    cpu_info.family = 4;
	} else {
	    cpu_info.family = 3;
	}
    }

    // Get the vendor name.
    if ( strcmp( cpu_info.name, "GenuineIntel" ) == 0 ) {
    	parsed_cpu.vendor = "GenuineIntel";
    } else if ( strcmp( cpu_info.name, "AuthenticAMD" ) == 0) {
	parsed_cpu.vendor = "AuthenticAMD";
    } else {
	parsed_cpu.vendor = "Unknown";
    }

    // Get the model name.
    cpu_get_modelname();
}

void AMD_K6_Write_msr( ulong msr, ulong v1, ulong v2, struct regs *regs ) {
    asm __volatile__ (
    	"pushfl\n"
        "cli\n"
        "wbinvd\n"
	"wrmsr\n"
	"popfl\n"
	: "=a" ( regs -> eax ), "=b" ( regs -> ebx ), "=c" ( regs -> ecx ),
	"=d" ( regs -> edx )
  	: "a" ( v1 ), "d" ( v2 ), "c" ( msr ) );
}

void AMD_K6_Read_msr( ulong msr, struct regs *regs ) {
    asm __volatile__ (
	"pushfl\n"
        "cli\n"
        "wbinvd\n"
        "xorl %%eax, %%eax\n"
        "xorl %%edx, %%edx\n"
	"rdmsr\n"
	"popfl\n"
	: "=a" ( regs -> eax ), "=b" ( regs -> ebx ), "=c" ( regs -> ecx ),
	"=d" ( regs -> edx ) : "c" ( msr ) );
}

void AMD_K6_Writeback( void ) {
    int mem;
    int c;
    struct regs regs;

    mem = ( getPhysicalMemoryMB() * 1024 ) / 4;
    if( !strcmp( parsed_cpu.vendor, "AuthenticAMD" ) ) {
	if ( cpu_info.family != 5 ) {
	    return;
	}

    	c = cpu_info.model;

    	/* model 8 stepping 0-7 use old style, 8-F use new style */
	if( cpu_info.model == 8 ) {
	    if( cpu_info.stepping < 8 ) {
		c = 7;
	    } else {
		c = 9;
	    }

	    switch ( c ) {
		/* old style Write back */
		case 6:
		case 7: {
		    AMD_K6_Read_msr( 0xC0000082, &regs );

		    if ( ( ( regs.eax >> 1 ) & 0x7F ) == 0 ) {
			printk( "\nAMD K6: WriteBack currently disabled\n" );
			printk( "\nAMD K6: Enabling WriteBack to %dMB\n",
			    mem * 4 );
			AMD_K6_Write_msr( 0xC0000082, ( ( mem << 1 ) & 0x7F ),
			    0, &regs );
		    } else {
			printk( "\nAMD K6: WriteBack currently enabled (%ldMB)\n",
			    ( ( regs.eax >> 1 ) & 0x7F ) * 4 );
		    }

		    break;
		}
		/* new style Write back */
		case 9: {
		    AMD_K6_Read_msr( 0xC0000082, &regs );
		    if ( ( ( regs.eax >> 22 ) & 0x3FF ) == 0 ) {
			printk( "\nAMD K6 : WriteBack Disabled\n" );
		    } else {
			printk( "\nAMD K6 : WriteBack Enabled (%ldMB)\n",
			    ( ( regs.eax >> 22 ) & 0x3FF ) * 4 );
		    }
		    printk( "\nAMD K6 : Enabled WriteBack (%dMB)\n", mem * 4 );
		    AMD_K6_Write_msr( 0xC0000082, ( ( mem << 22 ) & 0x3FF ),
			0, &regs );
		    break;
		}

		default: /* dont set it on Unknowns + k5's */ {
		    break;
		}
	    }
	}
    }
}

unsigned long cpu_mhz()
{
	return parsed_cpu.speed;
}

void init_cpu( void )
{
    getcpuinfo();
    printk( "\nProcessor:%s Family:%d Model:%d Stepping:%d",
	parsed_cpu.name, cpu_info.family, cpu_info.model, cpu_info.stepping );
    if ( parsed_cpu.speed != 0 ) {
	printk( "\nSpeed:%ld Mhz", parsed_cpu.speed );
	
	if ( cpu_info.cpuid ) {
	    if ( cpu_info.flags.flags.mmx ) {
		printk( ",MMX" );
	    }
	    if ( cpu_info.flags.flags.sse ) {
		printk( ",SSE" );
	    }
	    if ( cpu_info.flags.flags.amd_3dnow ) {
		printk( ",3DNOW" );
	    }
	}
    }

    if ( cpu_info.flags.flags.fpu ) {
	printk(" FPU:");
	if ( fpu_test() == 0 ) {
	    printk( "initialized" );
	} else {
	    printk( "failed" );
	}
    }

    AMD_K6_Writeback();
}


